import time, os, gc, sys, math,utime
from machine import PWM, FPIOA, Pin, UART
from media.sensor import *
from media.display import *
from media.media import *
import _thread
from pid import PID
from filter import LowPassFilter

'''
上下电机使用 270度舵机
左右电机使用 360度电机
'''


sensor = None

# 配置引脚
fpioa = FPIOA()
fpioa.set_function(11, FPIOA.UART2_TXD)
fpioa.set_function(12, FPIOA.UART2_RXD)
# 初始化UART2，波特率115200，8位数据位，无校验，1位停止位
uart = UART(UART.UART2, baudrate=115200, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE)

#指示灯
#####################################################################################
LED_G = Pin(20, Pin.OUT, pull=Pin.PULL_NONE, drive=7)  # 绿灯
# 定义闪烁函数
def blink_led(led):
    while True:
        led.value(1)  # 点亮LED
        time.sleep(0.2)  # 等待0.2秒
        led.value(0)  # 熄灭LED
        time.sleep(0.2)  # 等待0.2秒
# 创建并启动线程
_thread.start_new_thread(blink_led, (LED_G,))
#####################################################################################

#舵机
#####################################################################################
# 最小和最大占空比
min_duty = 2.5
max_duty = 12.5
mid_duty = 7.5  # 中间值，对应于0度
forward_speed = 2.5
stop_speed = 7.5
back_speed = 12.5
# 配置排针引脚号32，芯片引脚号为46的排针复用为PWM通道2输出
pwm_io2 = FPIOA()
pwm_io2.set_function(46, FPIOA.PWM2)
pwm_lr = PWM(2, 50, 50, enable=True)  # 配置PWM2，默认频率50Hz,占空比50%
pwm_lr.duty(stop_speed)    #舵机旋转到中间

# 配置排针引脚号12，芯片引脚号为47的排针复用为PWM通道3输出
pwm_io1 = FPIOA()
pwm_io1.set_function(47, FPIOA.PWM3)
pwm_ud = PWM(3, 50, 50, enable=True)  # 配置PWM3，默认频率50Hz,占空比50%
pwm_ud.duty(3)    #舵机旋转停止

# 将数值转换为占空比的函数
def input_to_duty_cycle(input_value):
    min_input = -max_duty
    max_input = max_duty
    min_duty_cycle = min_duty
    max_duty_cycle = max_duty

    # 确保输入值在允许的范围内
    if input_value < min_input or input_value > max_input:
        raise ValueError('输入值超出范围，应为{}到{}'.format(min_input,max_input))

    # 计算输出占空比
    output_value = min_duty_cycle + ((input_value - min_input) / (max_input - min_input)) * (max_duty_cycle - min_duty_cycle)
    return output_value


# 将数值转换为占空比的函数
def input_to_duty_cycle_lr(input_value):

    if input_value < 0:
        return back_speed
    elif input_value > 0:
        return forward_speed
    else:
        return stop_speed

#####################################################################################

# PID和滤波
#####################################################################################
lr_kp = 0.033#0.025 0.033
lr_ki = 0.003#0.0004 0.001
lr_kd = 0.29#0.35 0.29
pid_lr = PID(lr_kp, lr_ki, lr_kd,12.5,12.5)

ud_kp = 0.013
ud_ki = 0.00091
ud_kd = 0.02
pid_ud = PID(ud_kp, ud_ki, ud_kd,10,12.5)

alpha = 0.1  # 滤波器系数，可以根据需要调整这个值
lr_filter = LowPassFilter(alpha)
ud_filter = LowPassFilter(alpha)

#####################################################################################
# 常量定义
A4_WIDTH_MM = 297    # A4标准宽度（毫米）
A4_HEIGHT_MM = 210   # A4标准高度（毫米）
TARGET_RADIUS_MM = 40  # 目标物理半径（6cm = 60mm）

def calculate_dynamic_radius(current_w, current_h):
    """
    计算动态半径（单位：像素）
    :param current_w: 当前矩形宽度（像素）
    :param current_h: 当前矩形高度（像素）
    :return: 动态半径（像素）
    """
    # 计算宽高比例因子
    scale_w = current_w / A4_WIDTH_MM
    scale_h = current_h / A4_HEIGHT_MM

    # 取最大比例避免压缩变形
    scale = max(scale_w, scale_h)

    # 计算动态半径（物理尺寸恒为60mm）
    dynamic_radius = TARGET_RADIUS_MM * scale
    return int(dynamic_radius)  # 返回整数像素值


# 定义要识别颜色的阈值，这里需要根据你的具体情况调整，黑色里面找白色
# 你可以通过尝试不同的阈值来找到最适合你的物体颜色值
black_threshold =(77, 54, -15, 95, -37, 6)#(26, 100, -19, -128, -55, 71)
red_threshold = (1, 49, 23, -43, -31, 15)
try:

    # 初始化摄像头
    sensor = Sensor(id=2)
    # 传感器复位
    sensor.reset()
    # 开启镜像
    sensor.set_hmirror(True)#False
    # sensor vflip
    sensor.set_vflip(True)

    # 设置通道0的输出尺寸为	640x480
    sensor.set_framesize(Sensor.VGA, chn=CAM_CHN_ID_0)
    # 设置通道0的输出像素格式为RGB888
    sensor.set_pixformat(Sensor.RGB565, chn=CAM_CHN_ID_0)
    # 使用IDE显示图像
    Display.init(Display.VIRT, width=sensor.width(), height=sensor.height(), to_ide=True)

    # 初始化媒体管理器
    MediaManager.init()
    # 摄像头传感器开启运行
    sensor.run()


    fps = time.clock()

    while True:
        # 拍摄一张图片
        img = sensor.snapshot()

        fps.tick()

        blobs = img.find_blobs([black_threshold],pixels_threshold=2000, area_threshold=2000, merge=True)

        # 如果找到了至少一个blob
        if blobs:
           # 找到最大的blob
           largest_blob = max(blobs, key=lambda b: b.pixels())
           # 给标靶画框
           img.draw_rectangle(largest_blob.rect(), color=(255, 255, 255))
           # 过滤标靶的黑色胶带区域
           x = largest_blob.x() - 20
           y = largest_blob.y() - 20
           w = largest_blob.w() + 40
           h = largest_blob.h() + 40
           if x <= 0:
               x = 20
           if y <= 0:
               y = 20

           rad = calculate_dynamic_radius(w,h)
           img.draw_circle(x+w//2, y+h//2, rad, color=(255,255,255))

           # 设置要识别圆形标靶的识别感兴趣区域
           blobs_roi = [x, y, w, h]
           # 给要识别圆形标靶画框
           img.draw_rectangle(blobs_roi, color=(255, 255, 255))
           # 给要识别圆形标靶中心画十字
           img.draw_cross(x+w//2, y+h//2, color=(255, 255, 255), size=5, thickness=2)

           # 计算识别物与屏幕中心的误差
           x_offset = (x+w//2) - img.width() // 2

           y_offset = (y+h//2) - img.height() // 2

           # 屏幕显示位置信息和像素大小，包含正负号
           wz ="x={},y={}".format(x_offset,y_offset)
           img.draw_string_advanced(0,0,32,wz)

           pid_lr_value = pid_lr.pid_calc(0,x_offset)
           pid_ud_value = pid_ud.pid_calc(0,y_offset)

           duty_lr_value = input_to_duty_cycle(lr_filter.update(pid_lr_value))
           duty_ud_value = input_to_duty_cycle(ud_filter.update(pid_ud_value))

           zxc = "X偏移={}".format(duty_lr_value)
           print(zxc)


           pwm_lr.duty(duty_lr_value)
#           pwm_ud.duty(duty_ud_value)

        # 中心画十字
        img.draw_cross(img.width() // 2, img.height() // 2, color=(0, 255, 0), size=10, thickness=3)
        # IDE显示图片
        Display.show_image(img)
        #输出帧率
        #print(fps.fps())

except KeyboardInterrupt as e:
    print(f"user stop")
except BaseException as e:
    print(f"Exception '{e}'")
finally:
    # sensor stop run
    if isinstance(sensor, Sensor):
        sensor.stop()
    # deinit display
    Display.deinit()


    # release media buffer
    MediaManager.deinit()

    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)


